/*
 * Matrix.h
 *
 *  Created on: Feb 15, 2011
 *      Author: Anton
 */

#ifndef MATRIX_H_
#define MATRIX_H_

#include <stdarg.h>
#include <vector>
#include <iostream>
#include <math.h>
using std::ostream;
using std::endl;
using std::cout;

class Matrix {
private:
	int dx, dy; //Sizes of the matrix
	double **array; //Point to memory you will allocate using "new"

public:
	Matrix() {
	}

	Matrix(const Matrix &m) {
		dx = m.dx;
		dy = m.dy;

		array = new double*[dx];
		for (int i = 0; i < dx; i++) {
			array[i] = new double[dy];
		}

		for(int i=0; i<dx; i++) {
			for(int j=0; j<dy; j++) {
				array[i][j] = m.array[i][j];
			}
		}
	}

	Matrix(int size1, int size2, ...) {
		va_list arglist;
		va_start(arglist, size2);

		dx = size1;
		dy = size2;

		Matrix::array = new double*[dx];
		for (int i = 0; i < dx; i++) {
			array[i] = new double[dy];
		}

		for (int i = 0; i < size1; i++) {
			for (int j = 0; j < size2; j++) {
				array[i][j] = va_arg(arglist, double);
			}
		}

		va_end(arglist);
	}

	virtual ~Matrix() {
		delete array;
		array=NULL;
	}

	void print() {
		if((dx > 0) && (dy > 0)){
			for (int i = 0; i < dx; i++) {
				cout << "[ ";
				for (int j = 0; j < dy; j++) {
					cout << array[i][j] << " ";
				}
				cout << "]" << endl;
			}
		}
		else{
			cout << "null matrix" << endl;
		}
	}

	Matrix transMatrix() {
		Matrix T = Matrix(4,4, 1.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0, 0.0,0.0,1.0,0.0, 0.0,0.0,0.0,1.0);
		if(Matrix::getX()==1 && Matrix::getY()==4){
			double theta=Matrix::getvalue(0,3);
			double a=Matrix::getvalue(0,0);
			double alpha=Matrix::getvalue(0,1);
			double d=Matrix::getvalue(0,2);

			//Transformation Matrix utilizing DH Params
			T.setvalue(0,0,cos(theta));
			T.setvalue(0,1,-sin(theta)*cos(alpha));
			T.setvalue(0,2,sin(alpha)*sin(theta));
			T.setvalue(0,3,a*cos(theta));

			T.setvalue(1,0,sin(theta));
			T.setvalue(1,1,cos(alpha)*cos(theta));
			T.setvalue(1,2,-cos(theta)*sin(alpha));
			T.setvalue(1,3,a*sin(theta));

			T.setvalue(2,0,0.0);
			T.setvalue(2,1,sin(alpha));
			T.setvalue(2,2,cos(alpha));
			T.setvalue(2,3,d);
			//cout << "TransformMatrix: " << T << endl;
		}
		else{
			cout << "Error: Improper Matrix dimensions." << endl;
		}
		return T;
	}

	friend Matrix operator*( Matrix& lhs, Matrix& rhs);
	friend Matrix operator+( Matrix& lhs, Matrix& rhs);
	friend Matrix operator-( Matrix& lhs, Matrix& rhs);

	Matrix& operator=(const Matrix& rhs) {
		dx = rhs.dx;
		dy = rhs.dy;
		for (int i = 0; i < rhs.dx; i++) {
			for (int j = 0; j < rhs.dy; j++) {
				array[i][j] = rhs.array[i][j];
			}
		}
		return *(this);
	}

	void setvalue(int x, int y, double val) { array[x][y] = val; }
	double getvalue(int x, int y) { return array[x][y]; }
	int getX() { return dx; }
	int getY() { return dy; }

	int resize(int sizeX, int sizeY) {
		if (array != NULL) {
			delete[] array;
		}
		array = new double*[sizeX];
		for (int i = 0; i < sizeX; i++)
			array[i] = new double[sizeY];
		return 0;
	}

	friend ostream &operator<<(ostream &stream, Matrix &m);
};

Matrix operator*(Matrix& lhs, Matrix& rhs) {
	//ret is AxC size matrix
	//3x4 Matrix has 3 rows, 4 columns
	//dy, columns - dx, rows
	//(AxB)*(BxC) = (AxC)

	//Matrix:: is AxB
	//m is BxC

	Matrix ret = Matrix(rhs.dy, lhs.dx);

	if (lhs.dy == rhs.dx) {
		for (int i = 0; i < ret.getX(); i++) {
			for (int j = 0; j < ret.getY(); j++) {
				//Multiply/Add
				double value = 0;
				for (int k = 0; k < lhs.dy; k++) {
					value = value + lhs.getvalue(i, k) * rhs.getvalue(k, j);
				}
				ret.setvalue(i, j, value);
			}
		}
	} else {
		cout << "Matrix dimensions incorrect";
	}

	//ret.print();

	return ret;
}

Matrix operator-(Matrix& lhs, Matrix& rhs) {
	Matrix ret = Matrix(rhs.dy, rhs.dx);

	if ((lhs.dy == rhs.dy) & (lhs.dx == rhs.dx)) {
		for (int i = 0; i < ret.getX(); i++) {
			for (int j = 0; j < ret.getY(); j++) {
				double value = lhs.getvalue(i, j) - rhs.getvalue(i, j);
				ret.setvalue(i, j, value);
			}
		}
	} else {
		cout << "Matrix dimensions incorrect";
	}

	return ret;
}

Matrix operator+(Matrix& lhs, Matrix& rhs) {
	Matrix ret = Matrix(rhs.dy, rhs.dx);

	if ((lhs.dy == rhs.dy) & (lhs.dx == rhs.dx)) {
		for (int i = 0; i < ret.getX(); i++) {
			for (int j = 0; j < ret.getY(); j++) {
				double value = lhs.getvalue(i, j) + rhs.getvalue(i, j);
				ret.setvalue(i, j, value);
			}
		}
	} else {
		cout << "Matrix dimensions incorrect";
	}

	return ret;
}

ostream &operator<<(ostream &stream, Matrix &m) {

	for (int i = 0; i < m.dx; i++) {
		stream << "[ ";
		for (int j = 0; j < m.dy; j++) {
			stream << m.array[i][j] << " ";
		}
		stream << "]" << endl;
	}
	stream << endl;

	return stream;
}


#endif /* MATRIX_H_ */
